/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: CoordinateDriveClient.cpp,v 1.39 2009-10-23 13:56:43 morsko Exp $
 *
 * \file
 * \brief Implementation for CoordinateDrive interface client library.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * \todo Not yet complete.
 */

#include "CoordinateDriveClient.hpp"
#include <float.h>
#include <math.h>
//*****************************************************************************
using namespace MaCI::CoordinateDrive;
//*****************************************************************************
//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
CCoordinateDriveClient::CCoordinateDriveClient(gimi::GIMI *aGIMIPtr, const int aInterfaceMinor)
  : CSync(5,1), 
    CThread(1),
    CMaCI(aGIMIPtr, GIMI_PROTOCOL_MACI_COORDINATEDRIVE, aInterfaceMinor),
    iLastReachedPoint(),
    iCoordinates(),
    iServerStatusData(),
    iLatestPointSeq(-1),
    iCoordinatesSeq(-1),
    iServerStatusSeq(-1),
    iReplyMessages(),
    iCmdSequence(0),
    iCoordinateDriveClientCallback(NULL),
    iCallBackUserPtr(NULL)
{
}    
//*****************************************************************************
      
/** Destructor.
 */
CCoordinateDriveClient::~CCoordinateDriveClient()
{
  DoClose();
}
//*****************************************************************************      

/** Open client interface.
 */
MaCI::EMaCIError CCoordinateDriveClient::DoOpen(void){
  MaCI::EMaCIError result = MaCI::KMaCIError;

  if(!IsActive()){
    // Do Subs. using the MaCI provided function.
    result = SubscribeToInterface();

    // If all inits were fine...
    if(result == KMaCIOK){
      SetActive(true);
      // Enable sync. message replies.
      EnableSynchronousMessageReply();
      // Start the handler Thread
      CThread::RunThread();
      result = KMaCIOK;
    
      GetCoordinates(TimeoutValue(1000));
      dPrint(4,"CCoordinateDriveClient::DoOpen() was succesfull.");
    }
    
  }else{
    dPrint(3,"Client already opened, not Opening it again.");
    result = KMaCINotActive;
  }



 return result;


}

//*****************************************************************************

MaCI::EMaCIError CCoordinateDriveClient::DoClose(void)
{
  EMaCIError result = KMaCIError;

  if(IsActive()) {
    SetActive(false);
    
    // Wait for thread to complete.
    CThread::SetRequestTermination();
    
    result = UnsubscribeToInterface(TimeoutValue(2000));

    CSync::Notify(KEventPointReached);
    CSync::Notify(KReplyCoordinates);
    CSync::Notify(KReplyDriveModes);
    CSync::Notify(KEventCoordinates);
    CSync::Notify(KEventServerStatus);
    /* CThread::CancelThread(); //Removed by script */

    CThread::WaitThread();

  
    if(result == KMaCIOK){
      dPrint(4,"Succesfully closed CoordinateDriveClient");    
    }else{
      dPrint(4,"Unsubscribing failed!");
    }
  } else {
    dPrint(4, "CoordinateDriveClient not active, so not Closing.");
    result = KMaCINotActive;

  }
  
  return result;
}
//*****************************************************************************
    
bool CCoordinateDriveClient::SetCoordinate(float aX, 
					   float aY, 
					   float aA,
					   bool aIsDifferential,
					   bool aIsPathTargetPoint,
					   bool aIsWaitPoint)
{

  gim::binbag::CBinBag bb;
  CCoordinateDriveData cdData;
  cdData.EncodeTo(&bb);
  cdData.SetCommand(TCommand(KCommandSetCoordinate2D));
  
  cdData.SetCoordinate2D(TCoordinate2D(aX,
				       aY,
				       aA,
				       aIsDifferential,
				       aIsPathTargetPoint,
				       aIsWaitPoint));

  const MaCI::EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
						  bb.GetBufferSize());

  return (e == MaCI::KMaCIOK);

}
//*****************************************************************************

bool CCoordinateDriveClient::SetCoordinate(const TCoordinate2D &aCoordinate)
{
  gim::binbag::CBinBag bb;
  CCoordinateDriveData cdData;
  cdData.EncodeTo(&bb);
  cdData.SetCommand(TCommand(KCommandSetCoordinate2D));
  cdData.SetCoordinate2D(aCoordinate);

  const MaCI::EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
						  bb.GetBufferSize());

  return (e == MaCI::KMaCIOK);
}
//*****************************************************************************

bool CCoordinateDriveClient::RemoveCoordinates()
{
  gim::binbag::CBinBag bb;
  CCoordinateDriveData cdData;
  cdData.EncodeTo(&bb);
  cdData.SetCommand(TCommand(KCommandRemoveCoordinates));

  const MaCI::EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
						  bb.GetBufferSize());

  return (e == MaCI::KMaCIOK);
}
//*****************************************************************************
     
bool CCoordinateDriveClient::SetStart()
{
  gim::binbag::CBinBag bb;
  CCoordinateDriveData cdData;
  cdData.EncodeTo(&bb);
  cdData.SetCommand(TCommand(KCommandSetStart));

  const MaCI::EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
						  bb.GetBufferSize());

  return (e == MaCI::KMaCIOK);
}
//*****************************************************************************
      
bool CCoordinateDriveClient::SetStop()
{
  gim::binbag::CBinBag bb;
  CCoordinateDriveData cdData;
  cdData.EncodeTo(&bb);
  cdData.SetCommand(TCommand(KCommandSetStop));

  const MaCI::EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
						  bb.GetBufferSize());

  return (e == MaCI::KMaCIOK);
}
//*****************************************************************************
     
bool CCoordinateDriveClient::Continue()
{
  gim::binbag::CBinBag bb;
  CCoordinateDriveData cdData;
  cdData.EncodeTo(&bb);
  cdData.SetCommand(TCommand(KCommandContinue));

  const MaCI::EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
						  bb.GetBufferSize());

  return (e == MaCI::KMaCIOK);
}
//*****************************************************************************
   
bool CCoordinateDriveClient::SetDriveMode(ECoordinateDriveMode aMode)
{

  gim::binbag::CBinBag bb;
  CCoordinateDriveData cdData;
  cdData.EncodeTo(&bb);
  cdData.SetCommand(TCommand(KCommandSetDriveMode));
  cdData.SetDriveMode(TDriveMode(aMode));

  const MaCI::EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
						  bb.GetBufferSize());
 
  return (e == MaCI::KMaCIOK);
}
//*****************************************************************************

bool CCoordinateDriveClient::UnsetDriveMode(ECoordinateDriveMode aMode)
{

  gim::binbag::CBinBag bb;
  CCoordinateDriveData cdData;
  cdData.EncodeTo(&bb);
  cdData.SetCommand(TCommand(KCommandUnsetDriveMode));
  cdData.SetDriveMode(TDriveMode(aMode));

  const MaCI::EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
						  bb.GetBufferSize());

  return (e == MaCI::KMaCIOK);
}
//*****************************************************************************

bool CCoordinateDriveClient::GetLatestPointReached(TCoordinate2D &aCoordinate,
                                                   MaCI::Common::TTimestamp &aTimestamp,
						   int *aSeq,
						   int aTimeout_ms)
{
  bool result = false;

  Lock();
  
  if (aSeq == NULL) {
    if(iLatestPointSeq >=0){
      aCoordinate = *(iLastReachedPoint.GetCoordinate(0));
      aTimestamp = *(iLastReachedPoint.GetTimestamp());
      result = true;
    }
  }else{
    if (iLatestPointSeq <= *aSeq) {
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();
      do{
        Wait(time_left, KEventPointReached);
        time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
      }while(IsActive() && iLatestPointSeq < *aSeq && time_left > 0);

      if(iLatestPointSeq > *aSeq){
	aCoordinate = *(iLastReachedPoint.GetCoordinate(0));
        if(iLastReachedPoint.GetTimestamp() != NULL){
          aTimestamp = *(iLastReachedPoint.GetTimestamp());
        }
	*aSeq = iLatestPointSeq;
	result = true;


       }else{
	dPrint(4,"Failed to get latest point reached (Timeout %dms)",
	       aTimeout_ms);
      }
    }else{
      if(iLatestPointSeq > -1){
        aCoordinate = *(iLastReachedPoint.GetCoordinate(0));
        if(iLastReachedPoint.GetTimestamp() != NULL){
          aTimestamp = *(iLastReachedPoint.GetTimestamp());
        }
        *aSeq = iLatestPointSeq;
        result = true;
      }
    }
  }
  Unlock();
  return result;
	
}
//*****************************************************************************
bool CCoordinateDriveClient::GetCoordinates(unsigned int aTimeout_ms)
{
 // return result,
  bool result = false;

    
  // Construct command.
  CCoordinateDriveData cd;
  CCoordinateDriveData cdr;
  SMessageSource cdrms;
  cd.CreateInternalBinBag();
  cd.SetCommand(TCommand(KCommandGetCoordinates));
  cd.Print(1);
  // Get Unique MessageID
  const unsigned int messageId = GetUniqueMessageID();
  
  // Send command.
  SendInterfaceMessage(cd, messageId);

  // Wait for synchronous reply.
  EMaCIError e = ReceiveSynchronousMessageReply(cdr, cdrms, messageId, aTimeout_ms);
  if (e == KMaCIOK) {
    // Got reply.
    const TCoordinateHeader *ch = cdr.GetCoordinateHeader();
    if (ch) {
      iCoordinates = cdr;
      iCoordinatesSeq++;
    

      // Got headers, so the element must be intact.
      result = true;
    } else {
      dPrint(ODWARN,"Received reply without CoordinateHeader!");
      
    }
  }

  return result;
}
//*****************************************************************************
bool CCoordinateDriveClient::GetCoordinates(TCoordinateArray &aArray,
                                            MaCI::Common::TTimestamp &aTimestamp,
                                            int *aSeq,
					    unsigned int aTimeout_ms)
{
  bool result = false;

  Lock();
  aArray.clear();
  if (aSeq == NULL) {
    if(iCoordinatesSeq >=0){
      const MaCI::Common::TTimestamp *tstamp = iCoordinates.GetTimestamp();
      if(tstamp){
        aTimestamp = *tstamp;
      }else{
        MaCI::Common::TTimestamp t(0,0);
        aTimestamp = t;
      }
      const TCoordinateHeader *ch = iCoordinates.GetCoordinateHeader();
      if (ch) {
        for(unsigned int i = 0; i< ch->coordinateCount; ++i){
          // Get pointer to coordinate
          const TCoordinate2D *coord = iCoordinates.GetCoordinate(i);
          
          // Check the coordinate. (dirty way, as the Container should
          // have done this)
          assert(coord != NULL);
          
          // If reached here, push the coordinate to callers queue.
          aArray.push_back(*coord);
        }
        result = true;
      }
    }
  }else{
    if (iCoordinatesSeq <= *aSeq) {
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();
      
      Wait(time_left, KEventCoordinates);
      time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
      
      if(time_left > 0){
        const MaCI::Common::TTimestamp *tstamp = iCoordinates.GetTimestamp();
        if(tstamp){
          aTimestamp = *tstamp;
        }else{
          MaCI::Common::TTimestamp t(0,0);
          aTimestamp = t;
        }
        const TCoordinateHeader *ch = iCoordinates.GetCoordinateHeader();
        if (ch) {
          for(unsigned int i = 0; i< ch->coordinateCount; ++i){
            // Get pointer to coordinate
            const TCoordinate2D *coord = iCoordinates.GetCoordinate(i);
            
            // Check the coordinate. (dirty way, as the Container should
            // have done this)
            assert(coord != NULL);
            
            // If reached here, push the coordinate to callers queue.
            aArray.push_back(*coord);
          }
          *aSeq = iCoordinatesSeq;
          result = true;
        }


      }else{
	dPrint(4,"Failed to get latest coordinates (Timeout %dms)",
	       aTimeout_ms);
      }
    }else{
      if(iCoordinatesSeq > -1){
        const MaCI::Common::TTimestamp *tstamp = iCoordinates.GetTimestamp();
        if(tstamp){
          aTimestamp = *tstamp;
        }else{
          MaCI::Common::TTimestamp t(0,0);
          aTimestamp = t;
        }
        const TCoordinateHeader *ch = iCoordinates.GetCoordinateHeader();
        if (ch) {
          for(unsigned int i = 0; i< ch->coordinateCount; ++i){
            // Get pointer to coordinate
            const TCoordinate2D *coord = iCoordinates.GetCoordinate(i);
            
            // Check the coordinate. (dirty way, as the Container should
            // have done this)
            assert(coord != NULL);
            
            // If reached here, push the coordinate to callers queue.
            aArray.push_back(*coord);
          }
          *aSeq = iCoordinatesSeq;
          result = true;
        }
      }else{
	dPrint(4,"Failed to get latest coordinates (Timeout %dms)",
	       aTimeout_ms);
      }
    }
  }
  Unlock();
  return result;

 

}
//*****************************************************************************

bool CCoordinateDriveClient::GetServerStatusMessage(CCoordinateDriveData &aData,
                                                    MaCI::Common::TTimestamp &aTimestamp,
                                                    int *aSeq,
                                                    unsigned int aTimeout_ms)
{

  bool result = false;

  Lock();
  aData.Reset();
  if (aSeq == NULL) {
    if(iServerStatusSeq >=0){

      aData = iServerStatusData;
      const MaCI::Common::TTimestamp *tstamp = iServerStatusData.GetTimestamp();
      if(tstamp){
        aTimestamp = *tstamp;
      }else{
        MaCI::Common::TTimestamp t(0,0);
          aTimestamp = t;
      }
      *aSeq = iServerStatusSeq;
      result = true;
    }
  
  }else{
    if (iServerStatusSeq <= *aSeq) {
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();
      
      Wait(time_left, KEventServerStatus);
      time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
      
      if(time_left > 0){
      
        aData = iServerStatusData;
        const MaCI::Common::TTimestamp *tstamp = iServerStatusData.GetTimestamp();
        if(tstamp){
          aTimestamp = *tstamp;
        }else{
          MaCI::Common::TTimestamp t(0,0);
          aTimestamp = t;
        }
        *aSeq = iServerStatusSeq;
        result = true;



      }else{
	dPrint(4,"Failed to get latest server status (Timeout %dms)",
	       aTimeout_ms);
      }
    }else{
      if(iServerStatusSeq > -1){
        const MaCI::Common::TTimestamp *tstamp = iCoordinates.GetTimestamp();
        if(tstamp){
          aTimestamp = *tstamp;
        }else{
          MaCI::Common::TTimestamp t(0,0);
          aTimestamp = t;

        }
        aData = iServerStatusData;
        *aSeq = iServerStatusSeq;
        result = true;

      }else{
	dPrint(4,"Failed to get latest server status (Timeout %dms)",
	       aTimeout_ms);
      }
    }
  }
  Unlock();
  return result;


}
//*****************************************************************************

bool CCoordinateDriveClient::GetPathLength(float *aLinear, float *aRot)
{
  // \todo Fixed timeout value, should not be!
  float pathLength = 0.;
  float pathRot = 0.;
  int size;
  TCoordinateArray A;
  MaCI::Common::TTimestamp tstamp;
  bool ret;
  ret = GetCoordinates(A,tstamp,NULL, 1000);
  if (ret){
    size = A.size();
    for(int i = 1; i< size; i++){
      float x1 = A[i].x;
      float x2 = A[i-1].x;
      float y1 = A[i].y;
      float y2 = A[i-1].y;
      pathLength += sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
      pathRot += fabs(A[i].a-A[i-1].a);
    }
  }
  *aLinear = pathLength;
  *aRot = pathRot;
  return ret;
}
//*****************************************************************************

bool CCoordinateDriveClient::GetOpenDrivingModes(TDriveModeArray &aArray,
						 unsigned int aTimeout_ms)
{ 
  // return result,
  bool result = false;

  // Clear parameter array.
  aArray.clear();
  
  // Construct command.
  CCoordinateDriveData cd;
  CCoordinateDriveData cdr;
  SMessageSource cdrms;
  cd.CreateInternalBinBag();
  cd.SetCommand(TCommand(KCommandGetDrivingModes));
  
  // Get Unique MessageID
  const unsigned int messageId = GetUniqueMessageID();
  
  // Send command.
  SendInterfaceMessage(cd, messageId);

  // Wait for synchronous reply.
  EMaCIError e = ReceiveSynchronousMessageReply(cdr, cdrms, messageId, aTimeout_ms);
  if (e == KMaCIOK) {
    // Got reply.
    const TDriveModeHeader *dmh = cdr.GetDriveModeHeader();
    if (dmh) {
      for(unsigned int i = 0; i< dmh->driveModeCount; ++i){
        // Get pointer to coordinate
        const TDriveMode *dm = cdr.GetDriveMode(i);
        
        // Check the mode. (dirty way, as the Container should
        // have done this)
        assert(dm != NULL);
        
        // If reached here, push the coordinate to callers queue.
        aArray.push_back(*dm);
      }

      // Got headers, so the element must be intact.
      result = true;
    } else {
      dPrint(ODWARN,"Received reply without DriveModeHeader!");
      
    }
  }

  return result;
}
//*****************************************************************************

bool CCoordinateDriveClient::GetPlan(const TCoordinate2D &aCoordinateFrom,
                                     const TCoordinate2D &aCoordinateTo,
                                     TCoordinateArray &aPlan,
                                     unsigned int aTimeout_ms)
{

 // return result,
  bool result = false;

  // Clear parameter array.
  aPlan.clear();

 // Construct command.
  CCoordinateDriveData cd;
  CCoordinateDriveData cdr;
  SMessageSource cdrms;

  cd.CreateInternalBinBag();
  cd.SetCommand(TCommand(KCommandGetPlanFromTo));
  cd.SetCoordinateHeader(TCoordinateHeader(2));
  cd.SetCoordinate2D(aCoordinateFrom);
  cd.SetCoordinate2D(aCoordinateTo);


  // Get Unique MessageID
  const unsigned int messageId = GetUniqueMessageID();

  // Send command.
  SendInterfaceMessage(cd, messageId);

  // Wait for synchronous reply.
  EMaCIError e = ReceiveSynchronousMessageReply(cdr, cdrms, messageId, aTimeout_ms);

  if (e == KMaCIOK) {
    // Got reply.
    const TCoordinateHeader *header = cdr.GetCoordinateHeader();
    if(header){
      result = true;
      for(unsigned int i = 0; i< header->coordinateCount;i++){
        const TCoordinate2D *coord = cdr.GetCoordinate(i);
        if(coord){
          aPlan.push_back(*coord);
        }else{
          result = false;
          dPrint(3,"Warning: Invalid number of coordinates!");
        }
      }

    
    }else{
      //Only one point?
      const TCoordinate2D *coord = cdr.GetCoordinate(0);
      if(coord){
        aPlan.push_back(*coord);
      result = true;
      }else{
        dPrint(1,"No Coordinate in reply message");
        result = false;
      }
    }
  }else {
    dPrint(ODWARN,"No plan received!");
  }


  return result;
}
//*****************************************************************************

bool CCoordinateDriveClient::SetDrivingSpeed(const TSpeed aSpeed)
{
  gim::binbag::CBinBag bb;
  CCoordinateDriveData cdData;
  cdData.EncodeTo(&bb);
  cdData.SetCommand(TCommand(KCommandSetDrivingSpeed));
  
  cdData.SetSpeed(aSpeed);

  const MaCI::EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
						  bb.GetBufferSize());

  return (e == MaCI::KMaCIOK);


}
//*****************************************************************************

bool CCoordinateDriveClient::GetMaximumDrivingSpeed(TSpeed &aMaxDrivingSpeed,
                                                    unsigned int aTimeout_ms)
{

  // return result,
  bool result = false;

  
  // Construct command.
  CCoordinateDriveData cd;
  CCoordinateDriveData cdr;
  SMessageSource cdrms;
  cd.CreateInternalBinBag();
  cd.SetCommand(TCommand( KCommandGetMaxDrivingSpeed ));
  
  // Get Unique MessageID
  const unsigned int messageId = GetUniqueMessageID();
  
  // Send command.
  SendInterfaceMessage(cd, messageId);

  // Wait for synchronous reply.
  EMaCIError e = ReceiveSynchronousMessageReply(cdr, cdrms, messageId, aTimeout_ms);
  if (e == KMaCIOK) {
    // Got reply.

    if (cdr.GetCommand()->cmd == KCommandGetMaxDrivingSpeedReply) {
      if(cdr.GetSpeed()){
        aMaxDrivingSpeed = *(cdr.GetSpeed());

      
        // Got headers, so the element must be intact.
        result = true;
      }else{
        dPrint(3,"No speed container in data!");
        result = false;
      }
    } else {
      dPrint(ODWARN,"Received with wrong command (%d)!",cdr.GetCommand()->cmd);
      
    }
  }

  return result;


}
//*****************************************************************************

int CCoordinateDriveClient::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  EMaCIError e;
  gimi::GIMIMessage gmsg;
  
  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);

  dPrint(4, "GIMIMessage handler started");
   // Run as long as the thread should be active
  while(IsActive()) {
    // Call the ReceiveMessage wrapper with 1s timeout
    e = ReceiveInterfaceMessage(gmsg, 1000*60*60*24);
    if (e == KMaCIOK) {
      dPrint(8,"Message received from %08x",
             gmsg.getSenderId());
      ProcessMessage(gmsg);


    } else if (e != KMaCITimeout) {
      if(IsActive()){
        // Other error, try to recover by sleeping one second.
        dPrint(3,"ProcessMessage returned error %u and client still active. Trying to recover by sleeping 1000ms ",
               e);
        ownSleep_ms(1000);
      }else{
        //
        dPrint(3,"ProcessMessage returned error %u. Normal situtation because client is already closed",
               e);
      }
      
    }
  }
  
  dPrint(1, "GIMIMessage handler stopped");

  return result;
}
//*****************************************************************************

int CCoordinateDriveClient::ProcessMessage(const gimi::GIMIMessage &msg)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CCoordinateDriveData c;
  bool r;

 
  r = bb.Decode(msg.getData(), msg.getDatalength());

  if (r) {
    // Succesfully decoded as BinBag.
    r = c.DecodeFrom(&bb);
    if (r) {
      // Succesfully decoded as CoordinateDrive
      // Now, Check whether it was Command.
      const TCommand *cmd = c.GetCommand();
      if (cmd) {
        
        switch(cmd->cmd) {

	    //Getting a point reached event
        case KCommandArrivedToPointEvent:{
          dPrint(10,"Arrived to point event");
          Lock();
          iLastReachedPoint = c;
          ++iLatestPointSeq;
          Notify(KEventPointReached);
    
          Unlock();
    
          // Call the handler if set.
          if (iCoordinateDriveClientCallback) {
            dPrint(8,"Got Callback for CoordinateDriveDataEvent - executing!");
            iCoordinateDriveClientCallback->OnArrivedToPointEvent(c,
                iLatestPointSeq,
                *this);
            dPrint(8,"Callback for OnArrivedToPointEvent - execution complete!");
          
          }
          
          break;
        }
        case KCommandCoordinatesEvent:{
          dPrint(10,"Coordinate event");
          Lock();
          iCoordinates = c;
          ++iCoordinatesSeq;
          Notify(KEventCoordinates);

          Unlock();
          
          // Call the handler if set.
          if (iCoordinateDriveClientCallback) {
            dPrint(8,"Got Callback for CoordinateDriveDataEvent - executing!");
            iCoordinateDriveClientCallback->OnCoordinateEvent(c,
                iCoordinatesSeq,
                *this);
            dPrint(8,"Callback for CoordinateEvent - execution complete!");
          
          }
          
          break;
        }

        case KCommandUnableToDriveToCoordinate:
        case KCommandPathReady:
        case KCommandCalculating:{
          dPrint(10,"Server Status event");
          Lock();
          iServerStatusData = c;
          ++iServerStatusSeq;
          Notify(KEventServerStatus);

          Unlock();
          
          // Call the handler if set.
          if (iCoordinateDriveClientCallback) {
            dPrint(8,"Got Callback for ServerStatusEvent - executing!");
            iCoordinateDriveClientCallback->OnServerStatusMessage(c,
                                                                  iCoordinatesSeq,
                                                                  *this);
            dPrint(8,"Callback for ServerStatusEvent - execution complete!");
          
          }
          

          break;
        }
        default:
          dPrint(5,"Unidentified command! %d (%x)! Ignore.",
                 cmd->cmd, cmd->cmd);
          break;
        }
      }else{
        dPrint(5,"No command at message");
      }
    }else{
      dPrint(5,"DecodeFrom failed");
    }
  }else{
    dPrint(5,"Decode failed");
  }


  return result;
}
//*****************************************************************************
CCoordinateDriveClientCallback *CCoordinateDriveClient::GetCoordinateDriveClientCallBack(void) {
    return iCoordinateDriveClientCallback;
}
